#include<intersection_curve_of_two_surfaces.h>

#define DEFAULT_THRESHOLD  1e-7 
static inline int is_same_point(double* p1,double*p2)
{
    if(fabs(p1[0]-p2[0])<1e-6&&fabs(p1[1]-p2[1])<1e-6&&fabs(p1[2]-p2[2])<1e-6)
    {
        return 1;
    }
    return 0;
}
static inline double * get_coordinate_in_another_coordinate_system(double* p,double* a1,double *a2,double*a3)
{
    Matrix4x4* m=(Matrix4x4*)malloc(sizeof(Matrix4x4));
    Matrix4x4_init_double(m);

    double* data=(double*)(m->data);
    data[0*4+0]=a1[0]; data[1*4+0]=a1[1];  data[2*4+0]=a1[2];   
    data[0*4+1]=a2[0]; data[1*4+1]=a2[1];  data[2*4+1]=a2[2];
    data[0*4+2]=a3[0]; data[1*4+2]=a3[1];  data[2*4+2]=a3[2];
    Matrix4x4* mi=m->inverse(m);
    if(mi==NULL)
    {
        Matrix4x4_free(m);
        return NULL;
    } 
    data=(double*)(mi->data);
    double * re=(double*)malloc(sizeof(double)*3); 
    memset(re,0,sizeof(double)*3);
    for(int i=0;i<3;i++)
    {
        re[i]=0;
       for(int j=0;j<3;j++)
       {

            re[i]+=data[i*4+j]*p[j];
       } 

    }
    // printf("mat*************************\n");
    // Matrix4x4* mat=m->mult(m,mi);
    // for(int i=0;i<4;i++)
    // {
    //     for(int j=0;j<4;j++)
    //     {
    //         printf("%lf ",((double*)(mat->data))[i*4+j] );
    //     }
    //     printf("\n");
    // }
    // printf("mat *************************\n");

    Matrix4x4_free(m);
    Matrix4x4_free(mi);
    return re;
}
//return the triangle coordinate of p in p1 p2 p3
static inline double* get_triangle_coordinate(double* p,double * p1,double*p2,double* p3 )
{

    double a1[3]={p1[0]-p3[0],p1[1]-p3[1],p1[2]-p3[2]};
    double a2[3]={p2[0]-p3[0],p2[1]-p3[1],p2[2]-p3[2]};
    double pp[3]={p[0]-p3[0],p[1]-p3[1],p[2]-p3[2]};
    double* n=out_product(a1,a2);

    double* coord=get_coordinate_in_another_coordinate_system(pp,a1,a2,n);

    SAFE_FREE(n);
    if(coord==NULL)
    {
        return NULL;
    }
    double *re=(double*)malloc(sizeof(double)*3);
    re[0]=coord[0];re[1]=coord[1];
    

    re[2]=1-re[0]-re[1];
 
    SAFE_FREE(coord);
    return re;
}



template_v** get_intersection_vertices_from_two_cells(template_c* c1,template_c* c2)
{
    double ** points=get_intersection_from_two_cells(c1,c2);
    if(points==NULL)
    {
        return NULL;
    }
    int len=1;
    if(is_same_point(points[0],points[1]))
    {
        len=1;
    } 
    else
    {
        len=2;
    }

    template_v** re=(template_v**)malloc(sizeof(template_v*)*2);
    memset(re,0,sizeof(template_v*)*2);
    for(int i=0;i<len;i++)
    {
        re[i]=(template_v*)malloc(sizeof(template_v));
        Vertex_init_(re[i]);
        re[i]->point=(double*)malloc(sizeof(double)*3);
        re[i]->point[0]=points[i][0];re[i]->point[1]=points[i][1];re[i]->point[2]=points[i][2];
        
        template_c** cs=(template_c**)malloc(sizeof(template_c*)*2);
        cs[0]=c1;cs[1]=c2;
        re[i]->prop=cs;

    } 
    for(int i=0;i<2;i++)
    {

        SAFE_FREE(points[i]);
    } 
    free(points);

    return re;
}

static template_v* is_exist_same_point_in_cell(Mesh* m,template_c* c,double * p)
{
    for(auto it=m->cv_begin(m,*c);it!=m->cv_end(m,*c);it++)
    {
        for(Node* nit=(Node*)(quote(it)->prop);nit!=NULL;nit=(Node*)(nit->Next))
        {
            template_v* v=(template_v*)(nit->value);
            if(is_same_point(p,v->point))
            {
                return v;
            }
        }
    }

    for(auto it=m->chf_begin(m,*c);it!=m->chf_end(m,*c);it++)
    {

        template_f* f=(template_f*)(quote(it)->face);
        for(Node* nit=(Node*)(f->prop);nit!=NULL;nit=(Node*)(nit->Next))
        {
            template_v* v=(template_v*)(nit->value);
            if(is_same_point(p,v->point))
            {
                return v;
            }
        } 
    }


    for(Node* nit=(Node*)(c->prop);nit!=NULL;nit=(Node*)(nit->Next))
    {
        template_v* v=(template_v*)(nit->value);
        if(is_same_point(p,v->point))
        {
            return v;
        } 
    }


    return NULL;
}

static inline double distance_point_to_line_segments1(double* p,double* p1,double* p2)
{

    double n[3]={p2[0]-p1[0],p2[1]-p1[1],p2[2]-p1[2]};
    double a[3]={p[0]-p1[0],p[1]-p1[1],p[2]-p1[2]}; 
    if(!normalize(n)||!normalize(a))
    {
        printf("normalize cuowu\n");
    }   
    //double dis=a[0]*a[0]+a[1]*a[1]+a[2]*a[2];
    double dis1=n[0]*a[0]+n[1]*a[1]+n[2]*a[2];
    return dis1;
    
}
static void vertex_attachment_to_celln(template_c**cs,template_v* nv)
{
    Crossover_Point* cp=(Crossover_Point*)(nv->prop);
    Node* node=NULL;
    int flag=0;
    for(int i=0;i<2;i++)
    {
        flag=0;
          for(int j=0;j<cs[i]->vertices_size;j++)
          {
                if(is_same_point(nv->point,cs[i]->vertices[j]->point))
                {
                    cp->vs[i]=cs[i]->vertices[j];
                    node=(Node*)(cp->vs[i]->prop);
                    if(node!=NULL)
                    {
                        printf("cuowu :%d\n",nv->id );
                    }
                    node=node_overlying(node,nv);
                    cp->vs[i]->prop=node;
                    flag=1;
              //      printf("zhengque\n");
                    break;;
                }
          }
          if(flag)
          {
            continue;
          }
          for(Node* nit=cs[i]->halffaces;nit!=NULL;nit=(Node*)(nit->Next))
          {
                template_f* f=((template_hf*)(nit->value))->face;
                if(fabs(distance_point_to_line_segments1(nv->point,f->vertices[0]->point,f->vertices[1]->point)-1)<1e-7)
                {
                    cp->fs[i]=f;
                    node=(Node*)(f->prop);
                    node=node_overlying(node,nv);
                    f->prop=node; 
                    flag=1;
                   // printf("f\n");
                    break;
                }
          }
          if(flag)
          {
            continue;
          }
          cp->cs[i]=cs[i];
          node=(Node*)(cs[i]->prop);
          node=node_overlying(node,nv);
          cs[i]->prop=node;
    }
}

static inline  template_v* create_vertices_to_curve(Mesh* m,template_v*v)
{

    if(v==NULL)
    {
        return NULL;
    }
    Node* node=NULL;
    template_c** cs=(template_c**)(v->prop);
    template_v* v1=is_exist_same_point_in_cell(m,cs[0],v->point);
    template_v* re=NULL;

    if(v1==NULL)
    {
        template_v* nv=m->create_vertexv(m,v->point,3);
        Crossover_Point* cp=(Crossover_Point*)malloc(sizeof(Crossover_Point));
        crossover_point_init(cp);
        nv->prop=cp;
        re=nv; 
        
        vertex_attachment_to_celln(cs,nv);  
    }
    else
    {
        re= v1; 
    } 
    return re;  
}


static inline int is_intersected_two_cube(double *min1,double* max1,double* min2,double* max2)
{
    if(min2[0]>max1[0]+DEFAULT_THRESHOLD||min2[1]>max1[1]+DEFAULT_THRESHOLD||min2[2]>max1[2]+DEFAULT_THRESHOLD)
    {
        return 0;
    }
    if(max2[0]<min1[0]+DEFAULT_THRESHOLD||max2[1]<min1[1]+DEFAULT_THRESHOLD||max2[2]<min1[2]+DEFAULT_THRESHOLD) 
    {
        return 0;
    }
    return 1;
}
static inline double* get_cube_from_cell(template_c*c)
{
    double* re=(double*)malloc(sizeof(double)*6);
    re[0]=c->vertices[0]->point[0];re[1]=c->vertices[0]->point[1];re[2]=c->vertices[0]->point[2];
    re[3]=c->vertices[0]->point[0];re[4]=c->vertices[0]->point[1];re[5]=c->vertices[0]->point[2];

    for(int i=0;i<c->vertices_size;i++)
    {
        for(int j=1;j<3;j++)
        {
            if(c->vertices[j]->point[i]<re[i])
            {
                re[i]=c->vertices[j]->point[i];
            }
            else if(c->vertices[j]->point[i]>re[i+3])
            {
                re[i+3]=c->vertices[j]->point[i];
            }
        }

    }

    return re; 
}

static inline double* get_cube_from_cell_nodes(Node*n)
{
    if(n==NULL)
    {
        return NULL;
    }
    template_v* v1=(((template_c*)(n->value))->vertices[0]);
    double* re=(double*)malloc(sizeof(double)*6);
   
    re[0]=v1->point[0];re[1]=v1->point[1];re[2]=v1->point[2];
    re[3]=v1->point[0];re[4]=v1->point[1];re[5]=v1->point[2];
    for(Node* nit=n;nit!=NULL;nit=(Node*)(nit->Next))
    {
        template_c*  c=(template_c*)(nit->value);
        for(int i=0;i<c->vertices_size;i++)
        {
            for(int j=0;j<3;j++)
            {
                if(c->vertices[i]->point[j]<re[j])
                {
                    re[j]=c->vertices[i]->point[j];
                }
                else if(c->vertices[i]->point[j]>re[j+3])
                {
                    re[j+3]=c->vertices[i]->point[j];
                }

            } 

        } 
    }
    return re; 
 
}
//simplyfy n2
//@flag :: is changed
Node* simplify_node_of_two_nodes(Node* n1,Node *n2,int* flag)
{

    *flag=0;
    if(n1==NULL||n2==NULL)
    {
        return NULL;
    }
    Node* re=NULL;
    double* cube1=get_cube_from_cell_nodes(n1);
    for(Node* nit=n2;nit!=NULL;nit=(Node*)(nit->Next))
    {
        template_c* c=(template_c*)(nit->value);
        double* cube2=get_cube_from_cell(c);
        if(is_intersected_two_cube(cube1,&(cube1[3]),cube2,&(cube2[3])))
        {
            re=node_overlying(re,c);
        }
        else
        {
            *flag=1;
        }
        SAFE_FREE(cube2);  
    }
    SAFE_FREE(cube1);
    return re;
}




void get_intersection_lines_of_two_nodes(Node* n1,Node* n2,Mesh* m)
{
    if(n1==NULL||n2==NULL)
    {
        return;
    }
    m->simplex=1;
    m->manifold_require=0;
    for(Node* it1=n1;it1!=NULL;it1=(Node*)(it1->Next))
    {
        template_c* c1=(template_c*)(it1->value);
        for(Node* it2=n2;it2!=NULL;it2=(Node*)(it2->Next))
        {
            template_c*c2=(template_c*)(it2->value);
            template_v** vs=get_intersection_vertices_from_two_cells(c1,c2);

            if(vs==NULL)
            {

                continue;
            }
           
            template_v* vs_1[2]={NULL,NULL};
            for(int i=0;i<2;i++)
            { 
                vs_1[i]=create_vertices_to_curve(m,vs[i]);
                if(vs[i]!=NULL)
                {
                    free(vs[i]->prop);
                    free_Vertex(vs[i]);
                }
            } 
            if(vs_1[0]!=NULL&&vs_1[1]!=NULL)
            {
                m->create_facev(m,vs_1,2); 
            } 
           
            free(vs);
        }
    }
    //printf("%d\n",node_size(n2) );
  //  interset_jiance(m);
    // template_v v=m->get_vertex(m,406);
    // Crossover_Point*cp=(Crossover_Point*)(v.prop);
    // if(cp->fs[0]!=NULL)
    // {
    //     printf("f:%d\n",cp->fs[0]->id );
    // }
    // if(cp->cs[0]!=NULL)
    // {
    //     printf("c:%d\n",cp->cs[0]->id );
    // }
    // if(cp->vs[0]!=NULL)
    // {
    //     printf("v:%d\n",cp->vs[0]->id );
    // }
    //template_v* v1=cp->fs[0]->vertices[1];
    //v1=&v;
    //printf("%lf %lf %lf\n",v1->point[0],v1->point[1],v1->point[2] );

    // for(Node* nit=cp->cs[0]->halffaces;nit!=NULL;nit=(Node*)(nit->Next))
    // {
    //     template_f* f=((template_hf*)(nit->value))->face;
    //     printf("%lf \n", distance_point_to_line_segments1(v.point,f->vertices[0]->point,f->vertices[1]->point));
    // } 
}

void get_intersection_lines_of_two_nodesn(Node* n11,Node* n22,Mesh*m)
{
    Node* temp_n=NULL,* n1=node_copy(n11),*n2=node_copy(n22);
    int flag=0,flag1=0;
    while(1)
    {
        temp_n=simplify_node_of_two_nodes(n1,n2,&flag1);
        if(flag1==0)
        {
            flag++;
        }
        free_node(n2);
        n2=n1;n1=temp_n;temp_n=NULL;

        if(flag>=2)
        {
            break;
        }
    }  
    get_intersection_lines_of_two_nodes(n1,n2,m);
    m->manifold_require=0;
    free_node(n1);free_node(n2);
    //interset_jiance(m);
}
